// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//
// Author: v1.0 Yukihiro Saito
//

#include "multi_object_tracker/tracker/model/bicycle_tracker.hpp"

#include "multi_object_tracker/utils/utils.hpp"

// #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
// #include <tier4_autoware_utils/math/normalization.hpp>
// #include <tier4_autoware_utils/math/unit_conversion.hpp>

// #include <bits/stdc++.h>
// #include <tf2/LinearMath/Matrix3x3.h>
// #include <tf2/LinearMath/Quaternion.h>
// #include <tf2/utils.h>

// #ifdef ROS_DISTRO_GALACTIC
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #else
// #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
// #endif
// #include "object_recognition_utils/object_recognition_utils.hpp"

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace perception
{
    namespace algorithm
    {
        // using Label = autoware_auto_perception_msgs::msg::ObjectClassification;

        BicycleTracker::BicycleTracker(
            const double &time, const LidarObject &object)
            : Tracker(time, object),
              z_(object.z)
        {
            object_ = object;

            // Initialize parameters
            // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
            double r_stddev_x = 0.5;            // in vehicle coordinate [m]
            double r_stddev_y = 0.4;            // in vehicle coordinate [m]
            double r_stddev_yaw = 30 * deg2rad; // in map coordinate [rad]
            ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
            ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
            ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);

            // OBJECT SHAPE MODEL

            bounding_box_ = {
                object.length, object.width, object.height};
            // set minimum size
            bounding_box_.length = std::max(bounding_box_.length, 0.3);
            bounding_box_.width = std::max(bounding_box_.width, 0.3);
            bounding_box_.height = std::max(bounding_box_.height, 0.3);

            // Set motion model parameters
            {
                constexpr double q_stddev_acc_long =
                    9.8 * 0.35;                                 // [m/(s*s)] uncertain longitudinal acceleration
                constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration
                constexpr double q_stddev_yaw_rate_min = 5.0;   // [deg/s] uncertain yaw change rate, minimum
                constexpr double q_stddev_yaw_rate_max = 15.0;  // [deg/s] uncertain yaw change rate, maximum
                constexpr double q_stddev_slip_rate_min =
                    1.0; // [deg/s] uncertain slip angle change rate, minimum
                constexpr double q_stddev_slip_rate_max =
                    10.0;                               // [deg/s] uncertain slip angle change rate, maximum
                constexpr double q_max_slip_angle = 30; // [deg] max slip angle
                constexpr double lf_ratio = 0.3;        // [-] ratio of front wheel position
                constexpr double lf_min = 0.3;          // [m] minimum front wheel position
                constexpr double lr_ratio = 0.3;        // [-] ratio of rear wheel position
                constexpr double lr_min = 0.3;          // [m] minimum rear wheel position
                motion_model_.setMotionParams(
                    q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max,
                    q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio,
                    lr_min);
            }

            // Set motion limits
            {
                constexpr double max_vel = 80 / 3.6;              // [m/s] maximum velocity
                constexpr double max_slip = 30;                   // [deg] maximum slip angle
                motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle
            }

            // Set initial state
            {
                const double x = object.x;
                const double y = object.y;
                const double yaw = object.heading;
                auto pose_cov = object.pose_cov;
                double vel = 0.0;
                double vel_cov;
                const double &length = bounding_box_.length;

                // initial state covariance
                constexpr double p0_stddev_x = 0.8; // in object coordinate [m]
                constexpr double p0_stddev_y = 0.5; // in object coordinate [m]
                constexpr double p0_stddev_yaw =
                    25 * deg2rad; // in map coordinate [rad]
                constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0);
                constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0);
                constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);

                const double cos_yaw = std::cos(yaw);
                const double sin_yaw = std::sin(yaw);
                const double sin_2yaw = std::sin(2.0 * yaw);
                pose_cov[MSG_COV_IDX::X_X] =
                    p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw;
                pose_cov[MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw;
                pose_cov[MSG_COV_IDX::Y_Y] =
                    p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw;
                pose_cov[MSG_COV_IDX::Y_X] = pose_cov[MSG_COV_IDX::X_Y];
                pose_cov[MSG_COV_IDX::YAW_YAW] = p0_cov_yaw;

                constexpr double p0_stddev_vel = 1000 / 3.6; // in object coordinate [m/s]
                vel_cov = std::pow(p0_stddev_vel, 2.0);

                const double slip = 0.0;
                const double p0_stddev_slip = 5 * deg2rad; // in object coordinate [rad/s]
                const double slip_cov = std::pow(p0_stddev_slip, 2.0);

                // initialize motion model
                motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length);
            }
        }

        bool BicycleTracker::predict(const double &time)
        {
            return motion_model_.predictState(time);
        }

        LidarObject BicycleTracker::getUpdatingObject(
            const LidarObject &object)
        {
            LidarObject updating_object;

            // OBJECT SHAPE MODEL
            // convert to bounding box if input is convex shape

            updating_object = object;

            // UNCERTAINTY MODEL

            // fill covariance matrix
            auto &pose_cov = updating_object.pose_cov;
            pose_cov[MSG_COV_IDX::X_X] = ekf_params_.r_cov_x;       // x - x
            pose_cov[MSG_COV_IDX::X_Y] = 0;                         // x - y
            pose_cov[MSG_COV_IDX::Y_X] = 0;                         // y - x
            pose_cov[MSG_COV_IDX::Y_Y] = ekf_params_.r_cov_y;       // y - y
            pose_cov[MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw

            return updating_object;
        }

        bool BicycleTracker::measureWithPose(
            const LidarObject &object)
        {
            // get measurement yaw angle to update
            const double tracked_yaw = motion_model_.getStateElement(IDX::YAW);
            double measurement_yaw = 0.0;
            bool is_yaw_available = getMeasurementYaw(object, tracked_yaw, measurement_yaw);

            // update
            bool is_updated = false;
            {
                const double x = object.x;
                const double y = object.y;
                const double yaw = measurement_yaw;

                if (is_yaw_available)
                {
                    is_updated = motion_model_.updateStatePoseHead(
                        x, y, yaw, object.pose_cov);
                }
                else
                {
                    is_updated =
                        motion_model_.updateStatePose(x, y, object.pose_cov);
                }
                motion_model_.limitStates();
            }

            // position z
            constexpr double gain = 0.1;
            z_ = (1.0 - gain) * z_ + gain * object.z;

            return is_updated;
        }

        bool BicycleTracker::measureWithShape(
            const LidarObject &object)
        {

            constexpr double gain = 0.1;
            constexpr double gain_inv = 1.0 - gain;

            // update object size
            bounding_box_.length = gain_inv * bounding_box_.length + gain * object.length;
            bounding_box_.width = gain_inv * bounding_box_.width + gain * object.width;
            bounding_box_.height = gain_inv * bounding_box_.height + gain * object.height;
            // set minimum size
            bounding_box_.length = std::max(bounding_box_.length, 0.3);
            bounding_box_.width = std::max(bounding_box_.width, 0.3);
            bounding_box_.height = std::max(bounding_box_.height, 0.3);

            // update motion model
            motion_model_.updateExtendedState(bounding_box_.length);

            return true;
        }

        bool BicycleTracker::measure(
            const LidarObject &object, const double &time)
        {
            // keep the latest input object
            object_ = object;

            // update classification  TODO：选置信度最高的类别，根据过往的类别选还是当前的，策略未定
            const auto &current_classification = getClassification();
            // if (object.type == 5) // 未知
            // {
            //     setClassification(current_classification);
            // }

            // check time gap
            const double dt = motion_model_.getDeltaTime(time);
            if (0.01 /*10msec*/ < dt)
            {
                std::cout << "BicycleTracker::measure There is a large gap between predicted time and measurement time. :" << dt << std::endl;
                // RCLCPP_WARN(
                //     logger_,
                //     "BicycleTracker::measure There is a large gap between predicted time and measurement time. "
                //     "(%f)",
                //     dt);
            }

            // update object
            const LidarObject updating_object =
                getUpdatingObject(object);
            measureWithPose(updating_object);
            measureWithShape(updating_object);

            return true;
        }

        bool BicycleTracker::getTrackedObject(
            const double &time, LidarObject &object) const
        {
            object = object_;
            // TODO
            // object.id = getUUID();// 这里暂时不知道怎么赋值的
            // object.classification = getClassification();//类别暂时也不管
            Eigen::VectorXd pose, twist;
            pose << object.x, object.y, object.z, 0, 0, object.heading;
            twist << object.vx, object.vy, 0, 0, 0, object.angular_vz;
            auto &pose_cov = object.pose_cov;
            auto &twist_cov = object.twist_cov;

            // predict from motion model
            if (!motion_model_.getPredictedState(
                    time, pose, pose_cov, twist,
                    twist_cov))
            {
                std::cout << "BicycleTracker::getTrackedObject: Failed to get predicted state.\n";
                // RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state.");
                return false;
            }

            // position
            pose[2] = z_;
            object.x = pose[0];
            object.y = pose[1];
            object.z = pose[2];
            object.heading = pose[6];
            object.vx = twist[0];
            object.vy = twist[1];
            object.pose_cov = pose_cov;
            object.twist_cov = twist_cov;
            // set shape
            object.length = bounding_box_.length;
            object.width = bounding_box_.width;
            object.height = bounding_box_.height;
            const auto origin_yaw = object_.heading;
            const auto ekf_pose_yaw = pose[5];
            // 暂时不知道这是干嘛的 TODO
            // object.shape.footprint =
            //     tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);

            return true;
        }
    }
}